import rclpy
from rclpy.node import Node
from std_msgs.msg import Int32MultiArray
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from std_msgs.msg import Int32

import tf_transformations
import threading
import time
import math
from rclpy.executors import MultiThreadedExecutor

class AngleSubscriber(Node):
    def __init__(self):
        super().__init__('angle_subscriber')
        self.subscription = self.create_subscription(
            Int32MultiArray,
            'angle_topic',
            self.listener_callback,
            10
        )
        self.angle = 0
        self.lock = threading.Lock()
        self.trigger_event = threading.Event()

    def listener_callback(self, msg):
        with self.lock:
            if msg.data and len(msg.data) >= 2:
                self.angle = msg.data[0]
                if msg.data[1] == 1:
                    print("Trigger received! Angle:", self.angle)
                    self.trigger_event.set()

class OdomSubscriber(Node):
    def __init__(self):
        super().__init__('odom_subscriber')
        # 订阅里程计（用于获取当前朝向）
        self.odom_subscription = self.create_subscription(
            Odometry,
            'odom',
            self.odom_callback,
            10
        )
        self.current_yaw = 0.0  # 当前朝向
        self.lock = threading.Lock()

    def odom_callback(self, msg):
        """解析里程计数据，更新当前机器人朝向"""
        _q = msg.pose.pose.orientation
        _, _, yaw = tf_transformations.euler_from_quaternion([_q.x, _q.y, _q.z, _q.w])
        # print(f"里程计角度: {math.degrees(self.current_yaw) + 180}")
        with self.lock:
            self.current_yaw = yaw

class RotateRobot(Node):
    def __init__(self, odom_subscriber, angle_subscriber):
        super().__init__('rotate_robot')

        self.odom_subscriber = odom_subscriber  # 里程计订阅节点
        self.angle_subscriber = angle_subscriber  # 角度订阅节点
        self.cmd_vel_publisher = self.create_publisher(Twist, 'cmd_vel', 20)

    def rotate_to_angle(self):
        """让机器人旋转到目标角度"""
        with self.angle_subscriber.lock:
            audio_angle = self.angle_subscriber.angle  # 获取目标角度

        if audio_angle is None or audio_angle == 0 or audio_angle == 360:
            return

        with self.odom_subscriber.lock:
            odom_start_raw = math.degrees(self.odom_subscriber.current_yaw) + 180  # 获取当前朝向, 转换到 0 - 360 度

         # 计算旋转方向和目标角度
        if 0 < audio_angle < 180:  # 顺时针旋转
            direction = -1.0
            rotate_angle = audio_angle
        else:  # 逆时针旋转
            direction = 1.0
            rotate_angle = 360 - audio_angle

        # 里程计目标角度计算
        if direction < 0:
            odom_target_raw = odom_start_raw - rotate_angle
            if odom_target_raw < 0:
                odom_target_raw += 360
        else:
            odom_target_raw = odom_start_raw + rotate_angle
            if odom_target_raw > 360:
                odom_target_raw -= 360

        print(f"要旋转: {rotate_angle}°，方向: {direction}，当前角度: {odom_start_raw}°，目标角度: {odom_target_raw}°")

        full_error = odom_start_raw - odom_target_raw
        if full_error < 0:
            signed_flag = -1.0
        else:
            signed_flag = 1.0

        speed_base = 0.8
        twist = Twist()
        twist.angular.z = speed_base * direction
        self.cmd_vel_publisher.publish(twist)

        # time.sleep(rotate_time)

        while True:
            with self.odom_subscriber.lock:
                current_yaw = math.degrees(self.odom_subscriber.current_yaw) + 180  # 更新当前里程计角度

            error = current_yaw - odom_target_raw

            print(error)
            if abs(error) < 6:
                self.cmd_vel_publisher.publish(Twist())
                break

            time.sleep(0.08)
        # 停止机器人
        for i in range(0, 3):
            self.cmd_vel_publisher.publish(Twist())
        self.get_logger().info("Rotation complete.")

    def stop_move(self):
        # 停止机器人
        self.cmd_vel_publisher.publish(Twist())

def run_executor(executor):
    executor.spin()  # 运行 ROS 2 事件循环（不会阻塞主线程）



def main():
    rclpy.init()

    angle_sub_node = AngleSubscriber()  # 声源定位信息订阅
    odom_sub_node = OdomSubscriber()    # 里程计信息订阅
    node_rotate = RotateRobot(odom_sub_node, angle_sub_node)  # 传递订阅节点的引用

    # 多线程执行的管理器
    executor = MultiThreadedExecutor()
    executor.add_node(angle_sub_node)
    executor.add_node(odom_sub_node)
    executor.add_node(node_rotate)

    executor_thread = threading.Thread(target=run_executor, args=(executor,), daemon=True)
    executor_thread.start()

    try:
        while rclpy.ok():
            print("等待唤醒...")
            angle_sub_node.trigger_event.wait()
            angle_sub_node.trigger_event.clear()

            node_rotate.rotate_to_angle()  # 触发旋转
    except KeyboardInterrupt:
        print("程序终止")

    finally:
        node_rotate.stop_move()
        time.sleep(0.1)
        executor.shutdown()
        if rclpy.ok():  # 只有在 rclpy 仍然运行时才调用 shutdown
            rclpy.shutdown()

if __name__ == '__main__':
    main()
